/*
 * CommProtocol.cpp
 *
 *  Created on: Jan 15, 2014
 *      Author: ppks
 */

#include "common.h"
#include "CommProtocol.h"
#include "CommProtocol_Commands.h"
#include "Serial.h"
#include "FlightController.h"
#include "EEPROM.h"
#include "main.h"

/******************************************************************************/
/* Local Variables Declaration                                                */
/******************************************************************************/
static uint8_t currentSerialPort;
static uint8_t inputBuffer[INPUT_BUFFER_SIZE][UART_NUMBER];
static uint8_t checksum[UART_NUMBER];
static uint8_t indRX[UART_NUMBER];
static uint8_t cmdMSP[UART_NUMBER];

/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/
static void CommProtocol_ProcessSerial(void);
static void CommProtocol_EvaluateOtherData(uint8_t data);
static void CommProtocol_EvaluateHostCommand(void);
static void CommProtocol_ErrorReply(uint8_t size);
static void CommProtocol_SendHeaderResponse(uint8_t err, uint8_t size);
static void CommProtocol_TailReply(void);

/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/

/*******************************************************************************
* Function Name: CommProtocol_Process()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void CommProtocol_Process(void)
{
#if (COMM_PROTOCOL == COMM_SERIAL)
    /* Process communication on serial port */
    CommProtocol_ProcessSerial();
#else /* (COMM_PROTOCOL == COMM_SERIAL) */
    /* Currently do nothing as only UART is supported at the moment */
#endif /* (COMM_PROTOCOL == COMM_SERIAL) */

}

/*******************************************************************************
* Function Name: CommProtocol_ProcessSerial()
********************************************************************************
* Summary:
*  This function performs serial port communication.
*  It listens all available ports in the hardware and performs corresponding
*  actions based on the data or request received.
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void CommProtocol_ProcessSerial(void)
{
    uint8_t c;
    uint8_t n;
    static uint8_t offset[UART_NUMBER];
    static uint8_t dataSize[UART_NUMBER];
    static enum _serial_state {
        IDLE,
        HEADER_START,
        HEADER_M,
        HEADER_ARROW,
        HEADER_SIZE,
        HEADER_CMD,
    } c_state[UART_NUMBER]; /* = IDLE; */

    /* Check all ports for incoming data */
#if COMM_SERIAL_LISTEN_ALL_PORTS
    for(n = 0; n < UART_NUMBER; n++)
#else /* COMM_SERIAL_LISTEN_ALL_PORTS */
    n = COMM_PROTOCOL_SERIAL_PORT;
#endif /* COMM_SERIAL_LISTEN_ALL_PORTS */
    {
        currentSerialPort = n;

        /* Define condition for GPS to exclude its port from listening */
    #define GPS_COND
    #if defined(GPS_SERIAL_ENABLED)
        #if defined(GPS_PROMINI)
            #define GPS_COND
        #else
            #undef GPS_COND
            #define GPS_COND  && (GPS_SERIAL_PORT != currentSerialPort)
        #endif
    #endif

        /* Check if new data is available in the buffer and fet number of bytes available */
        uint8_t cc = CommSerial_IsNewDataAvailable(currentSerialPort);

        /* loop through all the bytes */
        while (cc-- GPS_COND)
        {
            /* indicates the number of occupied bytes in TX buffer */
            uint8_t bytesTXBuff = CommSerial_GetTXBuffSizeUsed(currentSerialPort);
            if (bytesTXBuff > TX_BUFFER_SIZE - 50 )
            {
                /* ensure there is enough free TX buffer to go further (50 bytes margin) */
                return;
            }

            /* Read Byte from serial interface */
            c = CommSerial_ReadByteFromBuffer(currentSerialPort);
      #ifdef SUPPRESS_ALL_SERIAL_MSP

            /* no MSP handling, so go directly */
            CommProtocol_EvaluateOtherData(c);
      #else
            /* regular data handling to detect and handle MSP and other data */
            if (c_state[currentSerialPort] == IDLE)
            {
                c_state[currentSerialPort] = (c == HEADER_START_BYTE) ? HEADER_START : IDLE;
                if (c_state[currentSerialPort] == IDLE)
                {
                    CommProtocol_EvaluateOtherData(c); // evaluate all other incoming serial data
                }
            }
            else if (c_state[currentSerialPort] == HEADER_START)
            {
                /* Header start byte is received, confirm the second byte of the header */
                c_state[currentSerialPort] = (c == HEADER_SECOND_BYTE) ? HEADER_M : IDLE;
            }
            else if (c_state[currentSerialPort] == HEADER_M)
            {
                /* Check if it is incoming command by evaluating third header byte */
                c_state[currentSerialPort] = (c == HEADER_DATA_IN) ? HEADER_ARROW : IDLE;
            }
            else if (c_state[currentSerialPort] == HEADER_ARROW)
            {
                /* Complete header is received, now evaluate the rest of the data */
                /* c should be a size of the packet at this stage, check if it is less than available input buffer */
                if (c > INPUT_BUFFER_SIZE)
                {
                    /* now we are expecting the payload size */
                    c_state[currentSerialPort] = IDLE;
                    continue;
                }

                dataSize[currentSerialPort] = c;
                offset[currentSerialPort] = 0;
                checksum[currentSerialPort] = 0;
                indRX[currentSerialPort] = 0;
                checksum[currentSerialPort] ^= c;          /* add size byte to the checksum */
                c_state[currentSerialPort] = HEADER_SIZE;  /* the command is to follow */
            }
            else if (c_state[currentSerialPort] == HEADER_SIZE)
            {
                /* Following byte should be a CMD */
                cmdMSP[currentSerialPort] = c;
                checksum[currentSerialPort] ^= c;
                c_state[currentSerialPort] = HEADER_CMD;
            }
            else if ((c_state[currentSerialPort] == HEADER_CMD) && (offset[currentSerialPort] < dataSize[currentSerialPort]))
            {
                checksum[currentSerialPort] ^= c;
                inputBuffer[offset[currentSerialPort]++][currentSerialPort] = c;
            }
            else if ((c_state[currentSerialPort] == HEADER_CMD) && (offset[currentSerialPort] >= dataSize[currentSerialPort]))
            {
                /* compare calculated and transferred checksum */
                if (checksum[currentSerialPort] == c)
                {
                    /* if  we got a valid packet, evaluate it */
                    CommProtocol_EvaluateHostCommand();
                }
                else
                {
                    /* Reply error to the host if the data was corrupted */
                    CommProtocol_ErrorReply(0);
                    CommProtocol_TailReply();
                }

                c_state[currentSerialPort] = IDLE;

                /* no more than one MSP per port and per cycle */
                cc = 0;
            }
      #endif /* SUPPRESS_ALL_SERIAL_MSP */
        }   /* While cc */
    }   /* For UARTNUM */
}

/*******************************************************************************
* Function Name: CommProtocol_EvaluateOtherData()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void CommProtocol_EvaluateOtherData(uint8_t data)
{
#ifndef SUPPRESS_OTHER_SERIAL_COMMANDS

#endif /* #ifndef SUPPRESS_OTHER_SERIAL_COMMANDS */
}

/*******************************************************************************
* Function Name: CommProtocol_EvaluateHostCommand()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void CommProtocol_EvaluateHostCommand(void)
{
#ifndef SUPPRESS_ALL_SERIAL_MSP

    switch (cmdMSP[currentSerialPort])
    {
       /* -------------------------------------------- */
       /* Data request commands: MSP starting with 1xx */
       /* -------------------------------------------- */
       case MSP_GET_IDENT_INFORMATION:
            CommProtocol_GetIdentInfoCMD();
            break;

        case MSP_GET_STATUS:
            CommProtocol_GetStatusCMD();
            break;

        case MSP_GET_RAW_IMU:
            CommProtocol_GetIMURawDataCMD();
            break;

        case MSP_GET_SERVO_DATA:
            CommProtocol_GetServoDataCMD();
            break;

        case MSP_GET_MOTOR_DATA:
            CommProtocol_GetMotorDataCMD();
            break;

        case MSP_GET_RC_DATA:
            CommProtocol_GetRCRawDataCMD();
            break;

        case MSP_GET_RAW_GPS_DATA:
            CommProtocol_GetGPSRawDataCMD();
            break;

        case MSP_GET_COMP_GPS_DATA:
            CommProtocol_GetGPSComputedDataCMD();
            break;

        case MSP_GET_ATTITUDE:
            CommProtocol_GetAttitudeCMD();
            break;

        case MSP_GET_ALTITUDE:
            CommProtocol_GetAltitudeCMD();
            break;

        case MSP_GET_ANALOG_DATA:
            CommProtocol_GetAnalogData();
            break;

        case MSP_GET_RC_TUNING:
            CommProtocol_GetRCTuningCMD();
            break;

        case MSP_GET_PID_SETTINGS:
            CommProtocol_GetPIDSettingsCMD();
            break;

        case MSP_GET_BOX_CONFIG:
            CommProtocol_GetBoxConfigCMD();
            break;

        case MSP_GET_MISC_DATA:
            CommProtocol_GetMiscDataCMD();
            break;

        case MSP_GET_MOTOR_PINS_CONFIG:
            CommProtocol_GetMotorPinsConfigCMD();
            break;

        case MSP_GET_BOX_ITEM_NAMES:
            CommProtocol_GetBoxItemNamesCMD();
            break;

        case MSP_GET_PID_NAMES:
            CommProtocol_GetPIDNames();
            break;

        case MSP_GET_WAYPOINTS_DATA:
            CommProtocol_GetWaypointsDataCMD();
            break;

        case MSP_GET_BOX_IDS:
            CommProtocol_GetBoxIdsCMD();
            break;

        case MSP_GET_SERVO_CONFIG:
            CommProtocol_GetServoConfigCMD();
            break;

        case MSP_GET_GPS_NAV_STATUS:
            CommProtocol_GetGPSNavStatusCMD();
            break;

        case MSP_GET_GPS_CONFIG:
            CommProtocol_GetGPSConfigCMD();
            break;

        /* -------------------------------------------- */
        /* Set parameter commands: MSP start with 2xx   */
        /* -------------------------------------------- */
        case MSP_SET_RAW_RC_DATA:
            CommProtocol_SetRCRawDataCMD();
            break;

        case MSP_SET_RAW_GPS_DATA:
            CommProtocol_SetGPSRawDataCMD();
            break;

        case MSP_SET_PID_SETTINGS:
            CommProtocol_SetPIDSettingsCMD();
            break;

        case MSP_SET_BOX_CONFIG:
            CommProtocol_SetBoxConfigCMD();
            break;

        case MSP_SET_RC_TUNING:
            CommProtocol_SetRCTuningCMD();
            break;

        case MSP_ACC_CALIBRATION:
            CommProtocol_RequestAccCalibrationCMD();
            break;

        case MSP_MAG_CALIBRATION:
            CommProtocol_RequestMagCalibrationCMD();
            break;

        case MSP_SET_MISC_DATA:
            CommProtocol_SetMiscDataCMD();
            break;

        case MSP_RESET_SYS_CONFIG:
            CommProtocol_ResetSystemConfigCMD();
            CommProtocol_HeaderReply(0);
            break;

        case MSP_SET_WAYPOINTS:
            CommProtocol_SetWaypointsCMD();
            break;

        case MSP_SELECT_SETTING_NUM:
            CommProtocol_SelectSettingsSetCMD();
            break;

        case MSP_SET_HEADING:
            CommProtocol_SetHeadingCMD();
            break;

        case MSP_SET_SERVO_CONFIG:
            CommProtocol_SetServoConfigCMD();
            break;

        case MSP_SET_MOTOR_DATA:
            CommProtocol_SetMotorDataCMD();
            break;

        case MSP_SET_GPS_CONFIG:
            CommProtocol_SetGPSConfigCMD();
            break;

        case MSP_BIND_CMD:
            CommProtocol_BindCMD();
            break;

        case MSP_EEPROM_WRITE_CMD:
            CommProtocol_EEPROMWriteCMD();
            break;

        /* -------------------------------------------- */
        /* Debug Message Commands: MSP start with 25x   */
        /* -------------------------------------------- */
        case MSP_GET_DEBUGMSG:
            CommProtocol_GetDebugMessageCMD();
            break;

        case MSP_GET_DEBUG_DATA:
            CommProtocol_GetDebugDataCMD();
            break;

        default:
            /* Don't know how to handle the command - reply zero length error */
            CommProtocol_ErrorReply(0);
            break;
    }

    /* Send tail response at the end */
    CommProtocol_TailReply();
#endif /* #ifndef SUPPRESS_ALL_SERIAL_MSP */
}

/*******************************************************************************
* Function Name: CommProtocol_SerialSendBlock()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void CommProtocol_SendBlock(uint8_t *data, uint8_t size)
{
    /* Send the header back */
    CommProtocol_HeaderReply(size);

    /* Append data bytes */
    while(size--)
    {
        CommProtocol_AppendUint8(*data++);
    }
}

/*******************************************************************************
* Function Name: CommProtocol_ReadBlock()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void CommProtocol_ReadBlock(uint8_t *data, uint8_t size)
{
    /* Send the zero length header reply */
    CommProtocol_HeaderReply(0);

    while(size--)
    {
        *data++ = CommProtocol_ReadUint8();
    }
}

/*******************************************************************************
* Function Name: CommProtocol_HeaderReply()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void CommProtocol_HeaderReply(uint8_t size)
{
    CommProtocol_SendHeaderResponse(0, size);
}

/*******************************************************************************
* Function Name: CommProtocol_ErrorReply()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void CommProtocol_ErrorReply(uint8_t size)
{
    CommProtocol_SendHeaderResponse(1, size);
}

/*******************************************************************************
* Function Name: CommProtocol_TailReply()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void CommProtocol_TailReply(void)
{
    /* append checksum byte at the end */
    CommProtocol_AppendUint8(checksum[currentSerialPort]);

#if (COMM_PROTOCOL == COMM_SERIAL)
    /* Transmit the data */
    CommSerial_SendData(currentSerialPort);
#endif /* #if (COMM_PROTOCOL == COMM_SERIAL)*/
}

/*******************************************************************************
* Function Name: CommProtocol_SerialSendHeader()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void CommProtocol_SendHeaderResponse(uint8_t err, uint8_t size)
{
    CommProtocol_AppendUint8(HEADER_START_BYTE);
    CommProtocol_AppendUint8(HEADER_SECOND_BYTE);
    CommProtocol_AppendUint8(err ? HEADER_DATA_ERROR: HEADER_DATA_OUT);

    /* Reset the checksum */
    checksum[currentSerialPort] = 0; /* start calculating a new checksum */

    CommProtocol_AppendUint8(size);
    CommProtocol_AppendUint8(cmdMSP[currentSerialPort]);
}

/*******************************************************************************
* Function Name: CommProtocol_AppendUint8()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void CommProtocol_AppendUint8(uint8_t data)
{
#if (COMM_PROTOCOL == COMM_SERIAL)
    /* append data to Serial Buffer */
    CommSerial_WriteByteToBuffer(currentSerialPort, data);
#endif /* COMM_PROTOCOL == COMM_SERIAL */

    /* add data to checksum */
    checksum[currentSerialPort] ^= data;
}

/*******************************************************************************
* Function Name: CommProtocol_AppendUint16()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void CommProtocol_AppendUint16(uint16_t data)
{
#if (COMM_PROTOCOL == COMM_SERIAL)
    /* append data to Serial Buffer */
    CommProtocol_AppendUint8(data & 0xFF);
    CommProtocol_AppendUint8((data >> 8) & 0xFF);
#endif /* COMM_PROTOCOL == COMM_SERIAL */
}

/*******************************************************************************
* Function Name: CommProtocol_AppendUint32()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void CommProtocol_AppendUint32(uint32_t data)
{
#if (COMM_PROTOCOL == COMM_SERIAL)
    /* append data to Serial Buffer */
    CommProtocol_AppendUint8(data & 0xFF);
    CommProtocol_AppendUint8((data >> 8) & 0xFF);
    CommProtocol_AppendUint8((data >> 16) & 0xFF);
    CommProtocol_AppendUint8((data >> 24) & 0xFF);
#endif /* COMM_PROTOCOL == COMM_SERIAL */
}

/*******************************************************************************
* Function Name: CommProtocol_ReadUint8()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
uint8_t CommProtocol_ReadUint8(void)
{
    return inputBuffer[indRX[currentSerialPort]++][currentSerialPort] & 0xff;
}

/*******************************************************************************
* Function Name: CommProtocol_ReadUint16()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
uint16_t CommProtocol_ReadUint16(void)
{
    uint16_t data = CommProtocol_ReadUint8();
    data += ((uint16_t)CommProtocol_ReadUint8() << 8);

    return(data);
}

/*******************************************************************************
* Function Name: CommProtocol_ReadUint32()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
uint32_t CommProtocol_ReadUint32(void)
{
    uint32_t data = CommProtocol_ReadUint16();
    data += ((uint32_t)CommProtocol_ReadUint16() << 16);

    return(data);
}

/*******************************************************************************
* Function Name: CommProtocol_SendNames()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void CommProtocol_SendNames(PGM_P s)
{
    /* Send Header Reply */
    CommProtocol_HeaderReply(strlen_P(s));

    /* Send names to the UART */
    for (PGM_P c = s; pgm_read_byte(c); c++)
    {
        CommProtocol_AppendUint8(pgm_read_byte(c));
    }
}
